#include <rtthread.h>
#include <board.h>

extern struct rt_semaphore *TimeoutGlobale;
extern struct mpu6xxx_device *MPU6050Globale;

#define FILTER_CNT          4

#define MAX(a,b) ((a) > (b) ? (a) : (b))
#define MIN(a,b) ((a) < (b) ? (a) : (b))
#define ABS(a) (0 - (a)) > 0 ? (-(a)) : (a)

#define MOST_ACTIVE_NULL                0        /*未找到最活跃轴*/
#define MOST_ACTIVE_X                   1        /*最活跃轴X*/
#define MOST_ACTIVE_Y                   2        /*最活跃轴Y*/
#define MOST_ACTIVE_Z                   3        /*最活跃轴Z*/

#define ACTIVE_PRECISION                60       /*活跃轴最小变化值*/
#define SAMPLE_SIZE   50
#define DYNAMIC_PRECISION               30       /*动态精度*/

#define DATDACOMMIT_UART__BUS_NAME          "uart7"  /* 上传数据设备名称 */
/*一个线性移位寄存器，用于过滤高频噪声*/
typedef struct {
  short x;
  short y;
  short z;
}axis_info_t;

typedef struct slid_reg{
    axis_info_t new_sample;
    axis_info_t old_sample;
}slid_reg_t;



typedef struct {
    axis_info_t newmax;
    axis_info_t newmin;
    axis_info_t oldmax;
    axis_info_t oldmin;
}peak_value_t;

static void peak_value_init(peak_value_t *peak);



typedef struct databuffer{
    rt_int16_t x;
    rt_int16_t y;
    rt_int16_t z;
}databuffer_t;

typedef struct filter_avg{
    axis_info_t info[FILTER_CNT];
    unsigned char count;
}filter_avg_t;

//读取xyz数据存入均值滤波器，存满进行计算，滤波后样本存入sample,如何读取存满就不多说了。
static void filter_calculate(filter_avg_t *filter, axis_info_t *sample)
{
    unsigned int i;
    short x_sum = 0, y_sum = 0, z_sum = 0;
    for (i = 0; i < FILTER_CNT; i++) {
        x_sum += filter->info[i].x;
        y_sum += filter->info[i].y;
        z_sum += filter->info[i].z;
    }
    sample->x = x_sum / FILTER_CNT;
    sample->y = y_sum / FILTER_CNT;
    sample->z = z_sum / FILTER_CNT;
}




//在动态阈值结构体初始化时，一定要将max的值都赋值为最小值，min赋值为最大值，这样才有利于动态更新。
static void peak_update(peak_value_t *peak, axis_info_t *cur_sample)
{
    static unsigned int sample_size = 0;
    sample_size ++;
    if (sample_size > SAMPLE_SIZE) {
        /*采样达到50个，更新一次*/
        sample_size = 1;
        peak->oldmax = peak->newmax;
        peak->oldmin = peak->newmin;
        //初始化
        peak_value_init(peak);
    }
    peak->newmax.x = MAX(peak->newmax.x, cur_sample->x);
    peak->newmax.y = MAX(peak->newmax.y, cur_sample->y);
    peak->newmax.z = MAX(peak->newmax.z, cur_sample->z);

    peak->newmin.x = MIN(peak->newmin.x, cur_sample->x);
    peak->newmin.y = MIN(peak->newmin.y, cur_sample->y);
    peak->newmin.z = MIN(peak->newmin.z, cur_sample->z);

}





static char slid_update(slid_reg_t *slid, axis_info_t *cur_sample)
{
    char res = 0;
    if (ABS((cur_sample->x - slid->new_sample.x)) > DYNAMIC_PRECISION) {
        slid->old_sample.x = slid->new_sample.x;
        slid->new_sample.x = cur_sample->x;
        res = 1;
    } else {
        slid->old_sample.x = slid->new_sample.x;
    }
    if (ABS((cur_sample->y - slid->new_sample.y)) > DYNAMIC_PRECISION) {
        slid->old_sample.y = slid->new_sample.y;
        slid->new_sample.y = cur_sample->y;
        res = 1;
    } else {
        slid->old_sample.y = slid->new_sample.y;
    }

    if (ABS((cur_sample->z - slid->new_sample.z)) > DYNAMIC_PRECISION) {
        slid->old_sample.z = slid->new_sample.z;
        slid->new_sample.z = cur_sample->z;
        res = 1;
    } else {
        slid->old_sample.z = slid->new_sample.z;
    }
    return res;
}




/*判断当前最活跃轴*/
static char is_most_active(peak_value_t *peak)
{
    char res = MOST_ACTIVE_NULL;
    short x_change = ABS((peak->newmax.x - peak->newmin.x));
    short y_change = ABS((peak->newmax.y - peak->newmin.y));
    short z_change = ABS((peak->newmax.z - peak->newmin.z));

    if (x_change > y_change && x_change > z_change && x_change >= ACTIVE_PRECISION) {
        res = MOST_ACTIVE_X;
    } else if (y_change > x_change && y_change > z_change && y_change >= ACTIVE_PRECISION) {
        res = MOST_ACTIVE_Y;
    } else if (z_change > x_change && z_change > y_change && z_change >= ACTIVE_PRECISION) {
        res = MOST_ACTIVE_Z;
    }
    return res;
}


/*判断是否走步*/
static int detect_step(peak_value_t *peak,slid_reg_t *slid)
{
      //char flag=0;
    int step_cnt = 0;
        char res=is_most_active(peak);
      switch (res)
      {
        case MOST_ACTIVE_NULL:

          break;

      case MOST_ACTIVE_X:
      {
        short threshold_x = (peak->oldmax.x+peak->oldmin.x)/2;
        if(slid->old_sample.x>threshold_x&&slid->new_sample.x<threshold_x)
        {
          //flag=1;
            step_cnt++;
        }
        break;
      }
      case MOST_ACTIVE_Y:
      {
          short threshold_y=(peak->oldmax.y+peak->oldmin.y)/2;
           if(slid->old_sample.y>threshold_y&&slid->new_sample.y<threshold_y)
           {
             //flag=1;
              step_cnt++;
           }
           break;
      }
      case MOST_ACTIVE_Z:
      {
          short threshold_z=(peak->oldmax.z+peak->oldmin.y)/2;
           if(slid->old_sample.z>threshold_z&&slid->new_sample.z<threshold_z)
           {
             //flag=1;
              step_cnt++;
           }
           break;
      }
      default:
        break;
      }
      return step_cnt;
}


//采样结构体初始化
static void peak_value_init(peak_value_t *peak)
{
      peak->newmax.x=0;
      peak->newmax.y=0;
      peak->newmax.z=0;

      peak->newmin.x=2048;
      peak->newmin.y=2048;
      peak->newmin.z=2048;

}


void user_thread_entry()
{
    axis_info_t   sample;
    axis_info_t   cur_sample;
    peak_value_t  peak;
    slid_reg_t    slid;
    databuffer_t databufferg,databuffera;

    static uint16_t Num;
    rt_device_t datacommit;
    struct rt_semaphore *timeout;
    rt_err_t error;
    struct mpu6xxx_device *MPU6050;


    rt_enter_critical();

    timeout = TimeoutGlobale;

    MPU6050 = MPU6050Globale;

    rt_exit_critical();

    //datacommit = rt_device_find(DATDACOMMIT_UART__BUS_NAME);

    peak_value_init(&peak);

        while(1)
        {
        rt_sem_take(timeout,1000);
        error = mpu6xxx_get_accel(MPU6050, &databuffera);
        sample.x= databuffera.x;
        sample.y= databuffera.y;
        sample.z= databuffera.z;


        peak_update(&peak,&sample);
        slid_update(&slid,&sample);

        Num += detect_step(&peak,&slid);

       // rt_kprintf("%d,%d,%d\n",sample.x,sample.y,sample.z);
        //rt_kprintf("%d\n",Num);
            //Display10BitData(GetData(GYRO_XOUT_H),2,1);   //显示X轴角速度
            //Display10BitData(GetData(GYRO_YOUT_H),7,1);   //显示Y轴角速度
            //Display10BitData(GetData(GYRO_ZOUT_H),12,1);  //显示Z轴角速度

        }

}

void start()
{
    rt_thread_t dynamic_thrad = RT_NULL;
    dynamic_thrad = rt_thread_create("step_count",
                                    user_thread_entry,
                                     RT_NULL,
                                     2048,
                                      16,
                                     500);
    if(dynamic_thrad != RT_NULL){
        rt_thread_startup(dynamic_thrad);
    }
}


INIT_APP_EXPORT(start);
